#include <ros/ros.h>
#include <geometry_msgs/Twist.h>
#include <std_msgs/Int32.h>
#include <math.h>

/// This node calculates the angle for the arduino board

/// actual, commanded and desired angle of the arduino board
double angle;
std_msgs::Int32 cmd_angle, angle_filtered;
double desired_angle, desired_angle_max;

/// Filter for the angle
int angle_old, angle_old_filtered;

/// Commanded speed of the base link
double xB_p;
double yB_p;
double qB_p;

double x;
double y;

/// max steering velocity of the arduino board
double q_p_max;

/// steering velocity of the arduino board
double q_p;

const static double EPS = 1e-2;


void angleCallback(const std_msgs::Int32::ConstPtr& msg)
{
   //angle = (msg->data)*M_PI/180; // angle in rad, msg in degree

   //angle_filtered.data = angle_old * 0.46f + angle_old_filtered * 0.54f;
   //angle_old_filtered = angle_filtered.data;
   //angle_old = angle*180.0/M_PI;
}

void cmdCallback(const geometry_msgs::TwistConstPtr &msg){
  xB_p = msg->linear.x;
  yB_p = msg->linear.y;
  qB_p = msg->angular.z;
}




int main(int argc, char** argv){

  ros::init(argc, argv, "arduino");

  ros::NodeHandle n;

  angle = 0.0;
  desired_angle = 0.0;
  desired_angle_max = M_PI_4; //rad
  cmd_angle.data = 0;

  angle_old = 0;
  angle_old_filtered = 0;
  angle_filtered.data = 0;

  xB_p = 0.0;
  yB_p = 0.0;
  qB_p = 0.0;

  ///Position to calculate the stear angle of the mobrob
  x = 0.3;
  y = 0.0;

  /// max steering velocity of the arduino board
  q_p_max = 1; // rad/sec

  q_p = 0.0;

  ros::Time current_time = ros::Time::now();
  ros::Time prev_time = ros::Time::now();

  ros::Publisher angle_pub = n.advertise<std_msgs::Int32>("/cmd_angle", 1);
  //ros::Publisher anglef_pub = n.advertise<std_msgs::Int32>("/poti_data_filtered", 1);

  //ros::Subscriber angle_sub = n.subscribe("/poti_data", 10, angleCallback);
  ros::Subscriber vel_sub = n.subscribe("/cmd_vel", 10, cmdCallback);


  ros::Rate r(200.0);
  while(n.ok()){
      ros::spinOnce();
      ros::Time current_time = ros::Time::now();


      if(xB_p > EPS || yB_p > EPS || qB_p > EPS || xB_p < -EPS || yB_p < -EPS || qB_p < -EPS){
      	/// Calculate only, if a command is set!
      	desired_angle = atan2(yB_p + qB_p * x, xB_p - qB_p * y);
      	if(desired_angle > M_PI_2 + 0.1){
      	  desired_angle = desired_angle - M_PI;
      	}
      	if(desired_angle < -M_PI_2 - 0.1){
      	  desired_angle = desired_angle + M_PI;
     	 }
      }

	if(desired_angle > desired_angle_max){
     	   desired_angle = desired_angle_max;
     	}
     	if(desired_angle < -desired_angle_max){
     	   desired_angle = -desired_angle_max;
     	}
	ROS_INFO("desired_angle: [%f]", desired_angle);

      //calculate the steering velocity for the arduino board
      q_p = 10*(desired_angle - angle);
      if(q_p > q_p_max){
        q_p = q_p_max;
      }

      if(q_p < -q_p_max){
        q_p = -q_p_max;
      }

	ROS_INFO("steering, velocity: [%f]", q_p);



      // fix step time of 0.1 sec
      //cmd_angle.data = (q_p * 0.1 + angle)*180.0/M_PI;

      //calculate the commanded angle for the arduino board
      cmd_angle.data = (q_p * (current_time - prev_time).toSec() + angle)*180.0/M_PI;
      angle = angle + q_p * (current_time - prev_time).toSec();

      ROS_INFO("cmd_angle: [%f]", angle);
      ROS_INFO("step time: [%f]", (current_time - prev_time).toSec());

      //cmd_angle.data = desired_angle*180.0/M_PI;

      //cmd_angle.data = 90*sin(current_time.toSec());

      angle_pub.publish( cmd_angle );
      //anglef_pub.publish(angle_filtered);

      prev_time = current_time;

      r.sleep();
  }
}

